#include "ctrl_step.hpp"
#include "communication.hpp"
#include "user_define.h"
#include "instances/dummy_robot.h"

extern DummyRobot dummy;

CtrlStepMotor::CtrlStepMotor(CAN_HandleTypeDef *_hcan, uint8_t _id, bool _inverse,
                             float _reduction, float _angleLimitMin, float _angleLimitMax) :
        nodeID(_id), hcan(_hcan), inverseDirection(_inverse), reduction(_reduction),
        angleLimitMin(_angleLimitMin), angleLimitMax(_angleLimitMax) {
    txHeader =
            {
                    .StdId = 0,
                    .ExtId = 0,
                    .IDE = CAN_ID_STD,
                    .RTR = CAN_RTR_DATA,
                    .DLC = 8,
                    .TransmitGlobalTime = DISABLE
            };
}


void CtrlStepMotor::SetEnable(bool _enable) {
    state = _enable ? FINISH : STOP;

    uint8_t mode = 0x01;
    txHeader.StdId = nodeID << 7 | mode;

    // Int to Bytes
    uint32_t val = _enable ? 1 : 0;
    auto *b = (unsigned char *) &val;
    for (int i = 0; i < 4; i++)
        canBuf[i] = *(b + i);

    CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}


void CtrlStepMotor::DoCalibration() {
    uint8_t mode = 0x02;
    txHeader.StdId = nodeID << 7 | mode;

    CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}


void CtrlStepMotor::SetCurrentSetPoint(float _val) {
    state = RUNNING;

    uint8_t mode = 0x03;
    txHeader.StdId = nodeID << 7 | mode;

    // Float to Bytes
    auto *b = (unsigned char *) &_val;
    for (int i = 0; i < 4; i++)
        canBuf[i] = *(b + i);

    CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}


void CtrlStepMotor::SetVelocitySetPoint(float _val) {
    state = RUNNING;

    uint8_t mode = 0x04;
    txHeader.StdId = nodeID << 7 | mode;

    // Float to Bytes
    auto *b = (unsigned char *) &_val;
    for (int i = 0; i < 4; i++)
        canBuf[i] = *(b + i);

    CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}


void CtrlStepMotor::SetPositionSetPoint(float _val) {
    uint8_t mode = 0x05;
    txHeader.StdId = nodeID << 7 | mode;

    // Float to Bytes
    auto *b = (unsigned char *) &_val;
    for (int i = 0; i < 4; i++)
        canBuf[i] = *(b + i);
    canBuf[4] = 1; // Need ACK

    CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}


void CtrlStepMotor::SetPositionWithVelocityLimit(float _pos, float _vel) {
    uint8_t mode = 0x07;
    txHeader.StdId = nodeID << 7 | mode;

    // Float to Bytes
    auto *b = (unsigned char *) &_pos;
    for (int i = 0; i < 4; i++)
        canBuf[i] = *(b + i);
    b = (unsigned char *) &_vel;
    for (int i = 4; i < 8; i++)
        canBuf[i] = *(b + i - 4);

    CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}


void CtrlStepMotor::SetNodeID(uint32_t _id) {
    uint8_t mode = 0x11;
    txHeader.StdId = nodeID << 7 | mode;

    // Int to Bytes
    auto *b = (unsigned char *) &_id;
    for (int i = 0; i < 4; i++)
        canBuf[i] = *(b + i);
    canBuf[4] = 1; // Need save to EEPROM or not

    CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}


void CtrlStepMotor::SetCurrentLimit(float _val) {
    uint8_t mode = 0x12;
    txHeader.StdId = nodeID << 7 | mode;

    // Float to Bytes
    auto *b = (unsigned char *) &_val;
    for (int i = 0; i < 4; i++)
        canBuf[i] = *(b + i);
    //canBuf[4] = 1; // Need save to EEPROM or not

    CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}


void CtrlStepMotor::SetVelocityLimit(float _val) {
    uint8_t mode = 0x13;
    txHeader.StdId = nodeID << 7 | mode;

    // Float to Bytes
    auto *b = (unsigned char *) &_val;
    for (int i = 0; i < 4; i++)
        canBuf[i] = *(b + i);
    canBuf[4] = 1; // Need save to EEPROM or not

    CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}


void CtrlStepMotor::SetAcceleration(float _val) {
    uint8_t mode = 0x14;
    txHeader.StdId = nodeID << 7 | mode;

    // Float to Bytes
    auto *b = (unsigned char *) &_val;
    for (int i = 0; i < 4; i++)
        canBuf[i] = *(b + i);
    canBuf[4] = 0; // Need save to EEPROM or not

    CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}


void CtrlStepMotor::ApplyPositionAsHome() {
    uint8_t mode = 0x15;
    txHeader.StdId = nodeID << 7 | mode;

    CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}


void CtrlStepMotor::SetEnableOnBoot(bool _enable) {
    uint8_t mode = 0x16;
    txHeader.StdId = nodeID << 7 | mode;

    // Int to Bytes
    uint32_t val = _enable ? 1 : 0;
    auto *b = (unsigned char *) &val;
    for (int i = 0; i < 4; i++)
        canBuf[i] = *(b + i);
    canBuf[4] = 1; // Need save to EEPROM or not

    CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}


void CtrlStepMotor::SetEnableStallProtect(bool _enable) {
    uint8_t mode = 0x1B;
    txHeader.StdId = nodeID << 7 | mode;

    uint32_t val = _enable ? 1 : 0;
    auto *b = (unsigned char *) &val;
    for (int i = 0; i < 4; i++)
        canBuf[i] = *(b + i);
    canBuf[4] = 1; // Need save to EEPROM or not

    CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}


void CtrlStepMotor::Reboot() {
    uint8_t mode = 0x7f;
    txHeader.StdId = nodeID << 7 | mode;

    CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}

void CtrlStepMotor::DeInitPos() {
    uint8_t mode = 0x7D;
    txHeader.StdId = nodeID << 7 | mode;

    CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}


void CtrlStepMotor::EraseConfigs() {
    uint8_t mode = 0x7e;
    txHeader.StdId = nodeID << 7 | mode;

    CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}


void CtrlStepMotor::SetAngle(float _angle) {
    _angle = inverseDirection ? -_angle : _angle;
    float stepMotorCnt = _angle / 360.0f * (float) reduction;
    SetPositionSetPoint(stepMotorCnt);
}


void CtrlStepMotor::SetAngleWithVelocityLimit(float _angle, float _vel) {
    _angle = inverseDirection ? -_angle : _angle;
    float stepMotorCnt = _angle / 360.0f * (float) reduction;


    if(dummy.HarVer == SITO){
        _vel = _vel*600;
        if((int32_t)_vel != (int32_t)LastSpeed && (int32_t)_vel != 0){
            //Respond(*usbStreamOutputPtr,"vel id:%d L:%d N:%d\n",nodeID,(int32_t)LastSpeed,(int32_t)_vel);
            RS485_Write(nodeID,0x002E,_vel);
            osDelay(15);
        }

        stepMotorCnt = stepMotorCnt * 32768;
        if((int32_t)stepMotorCnt != (int32_t)LastPos){
            Respond(*usbStreamOutputPtr,"pos id:%d L:%d N:%d\n",nodeID,(int32_t)LastPos,(int32_t)stepMotorCnt);
            RS485_Write(nodeID,0x0082,stepMotorCnt);
            osDelay(15);
        }

    }else
        SetPositionWithVelocityLimit(stepMotorCnt, _vel);
}

void CtrlStepMotor::MoveJointsWithRoundPerSec(float _joints,float RoundPerSec) {
    dummy.dynamicJointSpeeds.a[nodeID - 1] = RoundPerSec * (float) (dummy.motorJ[nodeID]->reduction); //r/s
    dummy.motorJ[nodeID]->SetAngleWithVelocityLimit(_joints - dummy.initPose.a[nodeID - 1],
                                               dummy.dynamicJointSpeeds.a[nodeID - 1]);
}


void CtrlStepMotor::UpdateAngle() {
    if(dummy.HarVer != SITO){
        uint8_t mode = 0x23;
        txHeader.StdId = nodeID << 7 | mode;

        CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
    }else{
        AskSITOAngle(nodeID);
    }

}

void CtrlStepMotor::UpdateMpuData() {
    uint8_t mode = 0x42;
    txHeader.StdId = nodeID << 7 | mode;

    CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}


void CtrlStepMotor::UpdateAngleCallback(float _pos, bool _isFinished) {
    state = _isFinished ? FINISH : RUNNING;
    float tmp;
    tmp = _pos / (float) reduction * 360;
    angle = inverseDirection ? -tmp : tmp;
}


void CtrlStepMotor::SetDceKp(int32_t _val) {
    uint8_t mode = 0x17;
    txHeader.StdId = nodeID << 7 | mode;

    auto *b = (unsigned char *) &_val;
    for (int i = 0; i < 4; i++)
        canBuf[i] = *(b + i);
    canBuf[4] = 1; // Need save to EEPROM or not

    CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}


void CtrlStepMotor::SetDceKv(int32_t _val) {
    uint8_t mode = 0x18;
    txHeader.StdId = nodeID << 7 | mode;

    auto *b = (unsigned char *) &_val;
    for (int i = 0; i < 4; i++)
        canBuf[i] = *(b + i);
    canBuf[4] = 1; // Need save to EEPROM or not

    CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}


void CtrlStepMotor::SetDceKi(int32_t _val) {
    uint8_t mode = 0x19;
    txHeader.StdId = nodeID << 7 | mode;

    auto *b = (unsigned char *) &_val;
    for (int i = 0; i < 4; i++)
        canBuf[i] = *(b + i);
    canBuf[4] = 1; // Need save to EEPROM or not

    CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}


void CtrlStepMotor::SetDceKd(int32_t _val) {
    uint8_t mode = 0x1A;
    txHeader.StdId = nodeID << 7 | mode;

    auto *b = (unsigned char *) &_val;
    for (int i = 0; i < 4; i++)
        canBuf[i] = *(b + i);
    canBuf[4] = 1; // Need save to EEPROM or not

    CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}


void CtrlStepMotor::SetDceKpKiKdKv(int16_t Kp,int16_t Ki,int16_t Kd,int16_t Kv) {
    uint8_t mode = 0x31;
    txHeader.StdId = nodeID << 7 | mode;

    *(uint16_t*)&canBuf[0] = Kp;
    *(uint16_t*)&canBuf[2] = Ki;
    *(uint16_t*)&canBuf[4] = Kd;
    *(uint16_t*)&canBuf[6] = Kv;

    CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}

void CtrlStepMotor::AskMotorSta() {
    uint8_t mode = 0x32;
    txHeader.StdId = nodeID << 7 | mode;

    CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}

void CtrlStepMotor::FindHomePosWithVelocity(float _val,bool updata,NEED_FIND_HOME_FLAG FindHomeWay) {
    state = RUNNING;

    uint8_t mode = 0x41;
    txHeader.StdId = nodeID << 7 | mode;

    // Float to Bytes
    *(float*)&canBuf[0] = _val;
    canBuf[4] = updata;
    canBuf[5] = FindHomeWay;
    if(NeedFindHomeFlag == FIND_HOME_WITH_PHYSICAL_LIMIT)
        canBuf[6] = dummy.FIND_PHYSICAL_LIMIT_MAX_CURRENT[nodeID]/100;
    else
        canBuf[6] = dummy.FIND_KEY_MAX_CURRENT[nodeID]/100;

    CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}

void CtrlStepMotor::ApplyPositionHomeInOneCrile() {
    uint8_t mode = 0x43;
    txHeader.StdId = nodeID << 7 | mode;


    CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}

void CtrlStepMotor::UpdateTofData() {
    uint8_t mode = 0x44;
    txHeader.StdId = nodeID << 7 | mode;

    CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}